#include "EthercatMaster.h"

namespace ECatDrv
{
char IOmap[4096];               // PDO映射内存
UINT mmRes = 0;                 // 用于交换数据的实时线程号
volatile int wkc;               //
volatile int wkc_expect;

// RT线程用于实时数据交换
void CALLBACK RTthread(UINT uTimerID, UINT uMsg, DWORD_PTR dwUser, DWORD_PTR dw1,  DWORD_PTR dw2)
{
    IOmap[0]++;
    wkc = ec_receive_processdata(EC_TIMEOUTRET);
//    if(wkc == 0){

//    }

    ec_send_processdata();
//    if(ec_send_processdata() == 0){

//    }
    /* do RT control stuff here */
}


EthercatMaster::EthercatMaster()
{
    // 复位主站状态
    m_masterState = masterState::M_IDLE;
    m_masterInited = false;
}

EthercatMaster::~EthercatMaster(){
//    if(mmRes != 0){
//        timeKillEvent(mmRes);        //释放定时器
//    }
    ec_close();
}

bool EthercatMaster::NetCardScan(std::vector<ECatDrv::netCardInfo> &card)
{
    bool res = false;
    card.clear();
    ec_adaptert *wpcap_adapter = ec_find_adapters();
    ec_adaptert *adapter = wpcap_adapter;
    while (adapter)
    {
        std::string adapter_key = adapter->desc;
        adapter_key = trim(adapter_key);
        adapter_key = adapter_key.empty() ? "Unknown" : adapter_key;
        netCardInfo tmp;
        tmp.name = adapter_key;
        tmp.id = adapter->name;
        card.push_back(tmp);
        res = true;
        adapter = adapter->next;
    }
    ec_free_adapters(wpcap_adapter);
    return res;
}


// 主站初始化
// 返回值:
// 0: 正常
// -1: ec初始化失败
// -2: 未扫描到从站
// -3: 有从站未处于SAFE_OP状态
// -4: 当前主站已经被初始化
int EthercatMaster::MasterInit(std::string cardId, std::vector<std::shared_ptr<EthercatSlave>> &slave,
                               std::vector<std::shared_ptr<ServoDrive>> &servo,
                               std::vector<std::shared_ptr<SafetyModule>> &safety)
{
    if(m_masterState != masterState::M_IDLE)
    {
        return -4;
    }

    if(!ec_init(cardId.c_str()))
    {
        return -1;
    }

    slave.clear();
    if (ec_config_init(FALSE) > 0)
    {
        // ec_slave的第0个设备无用，从1开始

        for(int slc = 1; slc <= ec_slavecount; slc++)
        {
            // 添加 if 语句识别更多自定义从站
//            if((ec_slave[slc].eep_man == ServoDevicedID_InoSV660N)
//                    && (ec_slave[slc].eep_id == ServoProductCode_InoSV660N))
//            {
//                auto ptr = std::make_shared<InoSV660NDrive>(ec_slave[slc].eep_man, ec_slave[slc].eep_id, ec_slave[slc].name, slc, S_ServoDrive);
//                slave.push_back(std::dynamic_pointer_cast<ECatDrv::EthercatSlave>(ptr));
//                servo.push_back(std::dynamic_pointer_cast<ECatDrv::ServoDrive>(ptr));
//            }
            if ((ec_slave[slc].eep_man == ServoDevicedID_xServo)
                    && ((ec_slave[slc].eep_id == ECatDrv::ServoProductCode_xServo_1
                         || ec_slave[slc].eep_id == ECatDrv::ServoProductCode_xServo_2
                         || ec_slave[slc].eep_id == ECatDrv::ServoProductCode_xServo_3
                         || ec_slave[slc].eep_id == ECatDrv::ServoProductCode_xDrive)))
            {
                auto ptr = std::make_shared<xServo>(ec_slave[slc].eep_man, ec_slave[slc].eep_id, ec_slave[slc].name, slc, S_ServoDrive);
                slave.push_back(std::dynamic_pointer_cast<ECatDrv::EthercatSlave>(ptr));
                servo.push_back(std::dynamic_pointer_cast<ECatDrv::ServoDrive>(ptr));
            }
            else
            {
                auto ptr = std::make_shared<UnknowSlave>(ec_slave[slc].eep_man, ec_slave[slc].eep_id, ec_slave[slc].name, slc, S_Other);
                slave.push_back(std::dynamic_pointer_cast<ECatDrv::EthercatSlave>(ptr));
                // 其他设备，暂时不做配置
            }
        }
    }
    else
    {
        ec_close();
        m_masterState = masterState::M_IDLE;
        return -2;
    }
    return 0;
}


// 主站配置
// 在 MasterInit 之后, 执行后所有从站处于 SAFE_OP 状态
int EthercatMaster::MasterConfig(std::vector<std::shared_ptr<ECatDrv::EthercatSlave>>& m_slave)
{
    // 判断是否扫描到从站设备
    if(ec_slavecount <= 0)
    {
        ec_close();
        return -2;
    }

    ec_slave[0].state = EC_STATE_PRE_OP;
    ec_writestate(0);
    // wait for all slaves to reach PRE_OP state
    if(ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE) == EC_STATE_PRE_OP)
    {
        // qDebug() << "preOp OK";
    }

    // ec_slave的第0个设备无用，从1开始
    for(int slc = 1; slc <= ec_slavecount; slc++)
    {
        EthercatSlave::registerSlave(slc, m_slave[slc - 1]);
    }

    // 配置映射内存，开启DC
    ec_config_map(&IOmap);
//    ec_configdc();

    // wait for safeop
    ec_slave[0].state = EC_STATE_SAFE_OP;
    ec_writestate(0);
    ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);

    // 检查状态
    if(false == ChkSlaveState(EC_STATE_SAFE_OP))
    {
        m_masterState = masterState::M_ERROR;
        return -3;
    }
    else
    {
        m_masterState = masterState::M_SAFE_OP;

    }
    m_masterInited = true;
    return 0;
}

// 检查是否所有从站都为设置的状态
bool EthercatMaster::ChkSlaveState(ec_state state)
{
    ec_readstate();
    for(int i = 1; i <= ec_slavecount; i++)
    {
        if(ec_slave[i].state != state)
        {
            return false;
        }
    }
    return true;
}

// 设置所有从站到OP状态
// 返回值 0:   正常
// 返回值 -1:  当前不处于SAFE_OP
// 返回值 -2:  未跳转到OP状态
int EthercatMaster::SetSlavesToOp(void)
{
    if(m_masterState != masterState::M_SAFE_OP)
    {
        return -1;
    }

    wkc_expect = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
    ec_slave[0].state = EC_STATE_OPERATIONAL;
    /* send one valid process data to make outputs in slaves happy*/
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    /* start RT thread as periodic MM timer */
    mmRes = timeSetEvent(PDO_PERIOD_MS, 1, RTthread, 0, TIME_PERIODIC);    // 需要>=2ms

    ec_writestate(0);
    int chk = 40;
    do{
        ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
    }
    while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));

    // 检查状态
    if(false == ChkSlaveState(EC_STATE_OPERATIONAL))
    {
        m_masterState = masterState::M_ERROR;
        return -2;
    }

    return 0;
}

// 设置所有从站到SafeOP状态
// 返回值 0:   正常
// 返回值 -1:  当前已经处于 SafeOP 状态
// 返回值 -2:  未跳转到SafeOP状态
int EthercatMaster::SetSlavesToSafeOp(void)
{
        if(m_masterState == masterState::M_SAFE_OP)
        {
            return -1;
        }

        ec_slave[0].state = EC_STATE_SAFE_OP;
        ec_writestate(0);
        ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);
        // 检查状态
        if(false == ChkSlaveState(EC_STATE_SAFE_OP))
        {
//            qDebug() << ec_slave[1].state;
            m_masterState = masterState::M_ERROR;
            return -2;
        }
        else
        {
            m_masterState = masterState::M_SAFE_OP;
        }
        if(mmRes)
        {
            timeKillEvent(mmRes);        //释放定时器
        }
        mmRes = 0;
        return 0;
}

/**
 * @brief 获取从站状态
 * @param state string类型的状态
 */
void EthercatMaster::GetAllSlaveState(std::vector<std::string> &state)
{
    state.clear();
    if(m_masterState == masterState::M_IDLE)
    {
        return;
    }
    ec_readstate();
    for(int i = 0; i <= ec_slavecount; i++)
    {
        if(ec_slave[i].state == EC_STATE_SAFE_OP)
        {
            state.push_back(std::string("SAFE_OP"));
        }
        else if(ec_slave[i].state == EC_STATE_OPERATIONAL)
        {
            state.push_back(std::string("OP"));
        }
        else if(ec_slave[i].state == EC_STATE_BOOT)
        {
            state.push_back(std::string("BOOT"));
        }
        else if(ec_slave[i].state == EC_STATE_PRE_OP)
        {
            state.push_back(std::string("PRE_OP"));
        }
        else if(ec_slave[i].state == EC_STATE_INIT)
        {
            state.push_back(std::string("INIT"));
        }
        else
        {
            state.push_back(std::string("OTHER"));
        }
    }
}

/**
 * @brief 获取从站状态
 * @param state masterState
 */
void EthercatMaster::GetAllSlaveState(std::vector<masterState> &state)
{
    state.clear();
    if(m_masterState == masterState::M_IDLE)
    {
        return;
    }

    ec_readstate();
    for(int i = 0; i <= ec_slavecount; i++)
    {
        if(ec_slave[i].state == EC_STATE_SAFE_OP)
        {
            state.push_back(masterState::M_SAFE_OP);
        }
        else if(ec_slave[i].state == EC_STATE_OPERATIONAL)
        {
            state.push_back(masterState::M_OP);
        }
        else if(ec_slave[i].state == EC_STATE_BOOT)
        {
            state.push_back(masterState::M_BOOT);
        }
        else if(ec_slave[i].state == EC_STATE_PRE_OP)
        {
            state.push_back(masterState::M_PRE_OP);
        }
        else if(ec_slave[i].state == EC_STATE_INIT)
        {
            state.push_back(masterState::M_INIT);
        }
        else
        {
            state.push_back(masterState::M_OTHER);
        }
    }
}



void EthercatMaster::writeSdo(int slave, int addr, int size, uint8_t *data)
{
    int __ret = ec_SDOwrite(slave, addr, 0x0, FALSE, size, data, EC_TIMEOUTTXM);
    //    qDebug() << "Slave: " << slave << " - Write at 0x" << hex << addr << dec << ", res = " << __ret;
    (void) __ret;
}



void EthercatMaster::readSdo(int slave, int addr, int &size, uint8_t *data)
{
    int __ret = ec_SDOread(slave, addr, 0x0, FALSE, &size, data, EC_TIMEOUTRXM);
    //    qDebug() << "Slave: " << slave << " - Read from 0x" << hex << addr << dec << ", size: " << size << ", res = " << __ret;
    (void) __ret;
}


std::string& EthercatMaster::trim(std::string &s)
{
    if (s.empty())
    {
        return s;
    }

    s.erase(0,s.find_first_not_of(" "));
    s.erase(s.find_last_not_of(" ") + 1);
    return s;
}

};
